
# Press Shift+F10 to execute it or replace it with your code.
# Press Double Shift to search everywhere for classes, files, tool windows, actions, and settings.


from Astar_WQP import MotionPlaner


if __name__ == '__main__':

    """
    ****data descripe the buildings or obstacles****
    data: [z, a_x,a_y, b_x,b_y, c_x,c_y, d_x,d_y, h]
        z is botttom of the building in z-anix
        a,b,c,d are vertices of rectangle， a = (a_x, a_y)
        h is the height
    """

    # 障碍物，第一个data_wall[0]内是不可穿墙，data_wall[1]内是可穿墙
    # data = [[0, 15, 47], [15, 49, 4], [[48, 10, 20, 12, 20, 10, -1, 12, -1, 50]]]
    data_wall = [[[15, 0, 20, 10, 20, 0, 27, 10, 27, 30]], [[11, 9, 4, 10, 4, 9, 15, 10, 15, 20]]]

    # 障碍物，第一个data_pipe[0]内是水管，data_pipe[1]内是风管
    data_pipe = [[[10, 3, 5, 4, 5, 3, 7, 4, 7, 30]], [[11, 9, 4, 10, 4, 9, 15, 10, 15, 20]]]

    start = [3, 15, 27]
    goal = [3, 28, 20]
    goal1 = [[3, 15, 12]]
    false_ceiling = 0
    motion_planer = MotionPlaner(data_wall, start, goal)  # 输入障碍物数据，起点，终点

    voxmap = motion_planer.plot3DModel(data_wall, data_pipe, start, goal, goal1, false_ceiling)






